#include "sabertoothdrive.h"
#include "main.h"
#define WHEEL_BACK 0
#define WHEEL_FOWRWAFD 1

//#define ADDRESS1 128


void autoBaud(void)
{
	uint8_t atuomatic_num=170;
	HAL_Delay(1500);
	printf("%c",atuomatic_num);
//	HAL_UART_Transmit(huart1, &atuomatic_num, 1, 30);
	HAL_Delay(2000);	
}

void command(uint8_t address1,uint8_t command_char,uint8_t value)
{
	char check_sum;
	check_sum=(address1+command_char+value)&127;
	
	printf("%c",address1);
	printf("%c",command_char);
	printf("%c",value);
	printf("%c",check_sum);

}

void throttleCommand(uint8_t addr,uint8_t command_char,signed char power)
{
  if(power>126)
		power=126;
	else if(power<-126)
		power=-126;
	
	if(power<0)
		power*=-1;
	
	command(addr,command_char,power);
	
}

void commandToWheel(uint8_t addr,uint8_t motor,signed char power)
{
	if(motor<1||motor>2)
		return;
	
	throttleCommand(addr,(motor==2?4:0)+(power<0?WHEEL_BACK:WHEEL_FOWRWAFD),power);

}

